# 从飞控数据中将RTK和机头信息保存进csv文件

from cmath import sqrt
import pandas as pd
import re
import csv
import math
import numpy as np

EARTH_RADIUS = 6371000

SCV_FILE_NAME = 'data_rtk.csv'
RTK_FILE_NMAE = 'rtk.log'
HGD_FILE_NAME = 'hdg.log'

a = 6378137
b = 6356752.3142
f = (a - b) / a
e_sq = f * (2 - f)
pi = 3.14159265359


def ecef_to_enu(x, y, z, lat0, lon0, h0):
    #print("ecef_to_enu")
    lamb = math.radians(lat0)
    phi = math.radians(lon0)
    s = math.sin(lamb)
    N = a / math.sqrt(1 - e_sq * s * s)

    sin_lambda = math.sin(lamb)
    cos_lambda = math.cos(lamb)
    sin_phi = math.sin(phi)
    cos_phi = math.cos(phi)

    x0 = (h0 + N) * cos_lambda * cos_phi
    y0 = (h0 + N) * cos_lambda * sin_phi
    z0 = (h0 + (1 - e_sq) * N) * sin_lambda

    xd = x - x0
    yd = y - y0
    zd = z - z0

    t = -cos_phi * xd - sin_phi * yd

    xEast = -sin_phi * xd + cos_phi * yd
    yNorth = t * sin_lambda + cos_lambda * zd
    zUp = cos_lambda * cos_phi * xd + cos_lambda * sin_phi * yd + sin_lambda * zd

    return xEast, yNorth, zUp


def geodetic_to_ecef(lat, lon, h):
    #print("geodetic_to_ecef")
    # (lat, lon) in degrees
    # h in meters
    lamb = math.radians(lat)
    phi = math.radians(lon)
    s = math.sin(lamb)
    N = a / math.sqrt(1 - e_sq * s * s)

    sin_lambda = math.sin(lamb)
    cos_lambda = math.cos(lamb)
    sin_phi = math.sin(phi)
    cos_phi = math.cos(phi)

    x = (h + N) * cos_lambda * cos_phi
    y = (h + N) * cos_lambda * sin_phi
    z = (h + (1 - e_sq) * N) * sin_lambda

    return x, y, z


def geodetic_to_enu(lat, lon, h, lat_ref, lon_ref, h_ref):
    #print("geodetic_to_enu")
    x, y, z = geodetic_to_ecef(lat, lon, h)

    return ecef_to_enu(x, y, z, lat_ref, lon_ref, h_ref)


def getDistance(x, y):
    return sqrt(x*x + y*y)


scv_file = open(SCV_FILE_NAME, 'w')
rtk_file = open(RTK_FILE_NMAE)

writer = csv.writer(scv_file)
writer.writerow(["RTKtime", "lat", "lon", "alt", "rx", "ry", "rz", "distance"])

land_lat, land_lon, land_alt, land_r = 30.339731981, 120.11556167, 47.447, 252.05
# land_lat, land_lon, land_alt, land_r = 30.3392134, 120.1154075, 48.770, 353.37
angle = (353.37 / 360) * (2 * pi)

pattern = re.compile(
    r'([0-9]+).([0-9]+).*?lat...([0-9]*)..lon...([0-9]*).*?alt.*?([0-9]*),')
for line in rtk_file.readlines():
    result = pattern.search(line)
    RTKtime = float(result.group(1) + '.' + result.group(2))
    lat_t: float = int(result.group(3)) / 10000000
    lon_t: float = int(result.group(4)) / 10000000
    alt_t: float = int(result.group(5)) / 1000
    rtk_x, rtk_y, rtk_z = geodetic_to_enu(lat_t, lon_t, alt_t, land_lat,
                                          land_lon, land_alt)

    # tvecXY = np.array([rtk_x, rtk_y])
    # rotate_matrix = np.array([[math.cos(angle), -math.sin(angle)],
    #                           [math.sin(angle),
    #                            math.cos(angle)]])
    # newTvecXY = np.dot(tvecXY, rotate_matrix)

    rtk_x = round(rtk_x, 3)
    rtk_y = round(rtk_y, 3)
    rtk_z = round(rtk_z, 3)

    distance = getDistance(rtk_x, rtk_y).real

    writer.writerow([
        RTKtime,
        result.group(3),
        result.group(4),
        result.group(5), rtk_x, rtk_y, rtk_z, distance
    ])

scv_file.close()
rtk_file.close()

hdg = []
rr = []

scv_file = pd.read_csv(SCV_FILE_NAME)
hdg_file = open(HGD_FILE_NAME)

rx = scv_file["rx"]
ry = scv_file["ry"]

pattern = re.compile(r'hdg.*?([0-9]*)}')
for line in hdg_file.readlines():
    result = pattern.search(line)
    rtk_r: float = land_r - (int(result.group(1)) / 100) - 90
    hdg.append(result.group(1))
    rr.append(rtk_r)

scv_file['hdg'] = hdg
scv_file['rr'] = rr
scv_file.to_csv(SCV_FILE_NAME, index=False, sep=',')

hdg_file.close()
